#include "BinaryHeap.h"
#include <memory>
#include <math.h>


BinaryHeap::BinaryHeap(void)
{
	heap.clear();
	hsize = 0;
}


BinaryHeap::~BinaryHeap(void)
{
	heap.clear();
}

void BinaryHeap::percolate_up( int idx )
{
	if (idx <= 1) return;
	int pIdx;

	if (idx%2 == 0)
		pIdx = idx/2;
	else
		pIdx = (idx-1)/2;

	if (heap[idx-1]->f < heap[pIdx-1]->f)
	{
		swap(idx,pIdx);
		percolate_up(pIdx);
	}
}

void BinaryHeap::percolate_down( int idx )
{
	uint32 lfIdx,rtIdx,minIdx;
	lfIdx = 2*idx;
	rtIdx = lfIdx + 1;
	if (rtIdx > heap.size())
	{
		if (lfIdx > heap.size())
			return;
		else
			minIdx = lfIdx;
	}
	else
	{
		if (heap[lfIdx-1]->f < heap[rtIdx-1]->f)
			minIdx = lfIdx;
		else 
			minIdx = rtIdx;
	}
	if (heap[minIdx-1]->f < heap[idx-1]->f)
	{
		swap(idx,minIdx);
		percolate_down(minIdx);
	}
}

bool BinaryHeap::empty()
{	
	return hsize==0;
}

void BinaryHeap::clear()
{
	heap.clear();
	hsize = 0;
}

void BinaryHeap::push( AStarNode* n )
{
	if (!n) return;
	//heap.insert(make_pair(++hsize,n));
	heap.push_back(n);
	hsize++;
	percolate_up(hsize);
}

AStarNode* BinaryHeap::pop()
{
	AStarNode* n=NULL;
	if (hsize>0)
	{
		n = heap[0];
		heap[0] = heap[hsize-1];
		heap.pop_back();
		hsize--;
		if (hsize>1)
			percolate_down(1);
	}
	return n;
}

void BinaryHeap::heapify( AStarNode* n )
{
	if(hsize == 0) return;
	if (n)
	{
		for (int i=1;i<=hsize;++i)
		{
			if (heap[i-1]==n)
			{
				percolate_down(i);
				percolate_up(i);
				return;
			}
		}
	}
	for (int i=floor(hsize/2);i>=1;--i)
	{
		percolate_down(i);
	}
}

void BinaryHeap::swap( int idx1,int idx2 )
{
	std::swap(heap[idx1-1],heap[idx2-1]);
}
